/** ###################################################################
**     Filename  : adc.c
**     Project   : adc
**     Processor : MC9S12XDT512BCFU
**     Version   : Driver 01.14
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 8.11.2011, 6:14
**     Abstract  :
**         Main module.
**         This module contains user's application code.
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/
/* MODULE adc */

/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "GPIO1.h"
#include "TI1.h"
#include "AD1.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"

/* User includes (#include below this line is not maintained by Processor Expert) */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>

//#define sign(x) ((x >= 0) ? 1.0 : -1.0)

#define LED0 PORTA_PA7
#define LED1 PORTA_PA6

#define CANE 0x80
#define INITAK 1
#define INITRQ 1
#define SLPRQ 2
#define SLPAK 2

/* CAN node identifier */
#define NODE_ID 6
/* Initialize counting number */
#define INIT_NUM 300
/* Moving average filter order */
#define MOV_AVG 200

typedef struct 
{
 word id;
 byte len;
 byte data[8];
} can_message_t;

typedef struct
{
  float Static;
  float Dynamic;
  float Temp1;
  float Temp2;
  float Density1;
  float Density2;
  float Height1;
  float Height2;
} constants;

typedef struct
{
  float DynamicBias;
  float LocalPress;
  float LocalTemp;
  float Density;
  float LastHeight;
} calculation;

typedef struct
{
  float Temp;
  float Static;
  float Dynamic;
  float AirSpeed;
  float VertSpeed;
  float Height;
} results;

typedef struct
{
  word dyn[MOV_AVG];
  word stat[MOV_AVG];
  word temp[MOV_AVG];
  unsigned int index;
  unsigned long RAW_Dyn;
  unsigned long RAW_Stat;
  unsigned long RAW_Temp;
  float Dyn;
  float Stat;
  float Temp;
} movingAverage;

/*typedef struct
{
  float p0;
  float p1;
  float p2;
  float p3;
} calCon;

typedef struct
{
  calCon statCon;
  float statOff1;
  float statOff2;
} calibrationStat;

typedef struct
{
  calCon statConM;
  calCon statConP;
} calibrationDyn;     */

byte can_recv( can_message_t *pmess);
byte can_send( can_message_t *pmess);
void can_init( void);
void initValues(void);

byte timerMeasure_int = 0;
constants Konst;
results oData;
movingAverage Average;
calculation Measure;
//calibrationStat CalibStatic;
//calibrationDyn CalibDynamic;

void main(void)
{
  /* Write your local variable definition here */
  byte ret;
  byte old;
  can_message_t can_msg;
  unsigned int freqDivide = 0;
  word measureDivider = 0;
  float hPa_Stat, hPa_Dyn;
  unsigned int i;

  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /*** End of Processor Expert internal initialization.                    ***/
  
  Cpu_Delay100US(0xe000);
  freqDivide = 0;
  initValues();
  can_init();

  /* Write your code here */
  for( ;;)
  {  
    // timer Measure interupt flag
    if(timerMeasure_int == 1)
    {
      timerMeasure_int = 0;
      freqDivide ++;
      measureDivider  ++;
      
      while(AD1_Measure(TRUE) != ERR_OK);
      if (ret == ERR_OK)
      {
        // odecet hodnot DA prevodniku
        ret = AD1_GetChanValue(0, (void*) (&(Average.dyn[Average.index])));
        ret = AD1_GetChanValue(1, (void*) (&(Average.temp[Average.index])));
        ret = AD1_GetChanValue(2, (void*) (&(Average.stat[Average.index])));
      }

	    if (Average.index >= MOV_AVG - 1)
      {
        Average.index = 0;
      }
      else
      {
        Average.index ++;
      }
	  
	    Average.RAW_Dyn = 0;
      Average.RAW_Stat = 0;
      Average.RAW_Temp = 0;
      
      for (i = 0; i < MOV_AVG; i++)
      {
        Average.RAW_Dyn  += Average.dyn[i];
        Average.RAW_Stat += Average.stat[i];
        Average.RAW_Temp += Average.temp[i];
      }     
	  
	    Average.Dyn  = (float)Average.RAW_Dyn  / (float)MOV_AVG;
      Average.Stat = (float)Average.RAW_Stat / (float)MOV_AVG;
      Average.Temp = (float)Average.RAW_Temp / (float)MOV_AVG;
      
	    oData.Static = Average.Stat * Konst.Static;
	    oData.Dynamic = (Average.Dyn - Measure.DynamicBias) * Konst.Dynamic;
	    oData.Temp = ((Average.Temp*Konst.Temp1) - 0.5) * Konst.Temp2;
	  
	    Measure.Density = Konst.Density1 - (Konst.Density2 * oData.Temp);
      oData.AirSpeed = sqrt((2*fabs(oData.Dynamic)) / Measure.Density);
      oData.Height = Konst.Height1 * ( 1.0 - pow(oData.Static / Measure.LocalPress, Konst.Height2));
      oData.VertSpeed = (oData.Height - Measure.LastHeight);
      Measure.LastHeight = oData.Height;
      
      if (freqDivide >= 305)
      {
        LED0 = ~LED0;
        freqDivide = 0;
      }
    } 
    
    // check can bus for a new byte
    ret = can_recv( &can_msg);
    if( ret == 1)
    {  
      CAN0RFLG = 0x01;     
      if( can_msg.id == 1)
      {
        LED1 = ~LED1;
        hPa_Stat = oData.Static / 100;
        hPa_Dyn  = oData.Dynamic / 100;
        
        can_msg.id = 332;
        can_msg.len = 8;
        can_msg.data[0] = NODE_ID;
        can_msg.data[1] = 2;
        can_msg.data[2] = 0;
        can_msg.data[3] = old;
        can_msg.data[4] = ((byte *)&(oData.Height))[0];
        can_msg.data[5] = ((byte *)&(oData.Height))[1];
        can_msg.data[6] = ((byte *)&(oData.Height))[2];
        can_msg.data[7] = ((byte *)&(oData.Height))[3];
        while( !can_send( &can_msg));
        
        can_msg.id = 314;
        can_msg.len = 8;
        can_msg.data[0] = NODE_ID;
        can_msg.data[1] = 2;
        can_msg.data[2] = 0;
        can_msg.data[3] = old;
        can_msg.data[4] = ((byte *)&(oData.VertSpeed))[0];
        can_msg.data[5] = ((byte *)&(oData.VertSpeed))[1];
        can_msg.data[6] = ((byte *)&(oData.VertSpeed))[2];
        can_msg.data[7] = ((byte *)&(oData.VertSpeed))[3];
        while( !can_send( &can_msg));
        
        can_msg.id = 315;
        can_msg.len = 8;
        can_msg.data[0] = NODE_ID;
        can_msg.data[1] = 2;
        can_msg.data[2] = 0;
        can_msg.data[3] = old;
        can_msg.data[4] = ((byte *)&(oData.AirSpeed))[0];
        can_msg.data[5] = ((byte *)&(oData.AirSpeed))[1];
        can_msg.data[6] = ((byte *)&(oData.AirSpeed))[2];
        can_msg.data[7] = ((byte *)&(oData.AirSpeed))[3];
        while( !can_send( &can_msg));
        
        can_msg.id = 324;
        can_msg.len = 8;
        can_msg.data[0] = NODE_ID;
        can_msg.data[1] = 2;
        can_msg.data[2] = 0;
        can_msg.data[3] = old;
        can_msg.data[4] = ((byte *)&(oData.Temp))[0];
        can_msg.data[5] = ((byte *)&(oData.Temp))[1];
        can_msg.data[6] = ((byte *)&(oData.Temp))[2];
        can_msg.data[7] = ((byte *)&(oData.Temp))[3];
        while( !can_send( &can_msg));
        
        can_msg.id = 325;
        can_msg.len = 8;
        can_msg.data[0] = NODE_ID;
        can_msg.data[1] = 2;
        can_msg.data[2] = 0;
        can_msg.data[3] = old;
        can_msg.data[4] = ((byte *)&(hPa_Dyn))[0];
        can_msg.data[5] = ((byte *)&(hPa_Dyn))[1];
        can_msg.data[6] = ((byte *)&(hPa_Dyn))[2];
        can_msg.data[7] = ((byte *)&(hPa_Dyn))[3];
        while( !can_send( &can_msg));
        
        can_msg.id = 326;
        can_msg.len = 8;
        can_msg.data[0] = NODE_ID;
        can_msg.data[1] = 2;
        can_msg.data[2] = 0;
        can_msg.data[3] = old;
        can_msg.data[4] = ((byte *)&(hPa_Stat))[0];
        can_msg.data[5] = ((byte *)&(hPa_Stat))[1];
        can_msg.data[6] = ((byte *)&(hPa_Stat))[2];
        can_msg.data[7] = ((byte *)&(hPa_Stat))[3];
        while( !can_send( &can_msg));
        
        old++;
      }
    }
  }

  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

void initValues(void)
{
  int i;
  byte ret;
  word dyn = 0;
  word stat = 0;
  word temp = 0;
  unsigned long long Dynamic = 0;
  unsigned long long Static = 0;
  unsigned long long Temp = 0;
  unsigned long NumDyn = 0;
  unsigned long NumStat = 0;
  unsigned long NumTemp = 0;

  Measure.Density     = 0;
  Measure.LocalTemp   = 0;
  Measure.LastHeight  = 0;
  Measure.LocalPress  = 0;
  Measure.DynamicBias = 0;

  oData.Temp      = 0;
  oData.Static    = 0;
  oData.Dynamic   = 0;
  oData.AirSpeed  = 0;
  oData.VertSpeed = 0;
  oData.Height    = 0;
  
  // vypocet lokalniho statickeho a dynamickeho tlaku a teploty
  for (i = 0; i < INIT_NUM; i++)
  {
    while(AD1_Measure(TRUE) != ERR_OK);
      
    ret = AD1_GetChanValue(0, (void*) (&dyn));
    if (ret == ERR_OK)
    {
      Dynamic += dyn;
      NumDyn ++;
    }
    ret = AD1_GetChanValue(1, (void*) (&temp));
    if (ret == ERR_OK)
    {
      Temp    += temp;
      NumTemp ++;
    }
    ret = AD1_GetChanValue(2, (void*) (&stat));
    if (ret == ERR_OK)
    {
      Static  += stat;
      NumStat ++;
    }
  }
  
  // konstanta pro prevod z cisla na Pa pro senzor statickeho tlaku
  Konst.Static   = 5.12*115000.0/4.8/1024.0 * 1.24;
  // konstanta pro prevod z cisla na Pa pro senzor dynamickeho tlaku
  Konst.Dynamic  = 5.12*4000.0/4.8/1024.0;
  // konstanta pro prevod z cisla na C pro senzor teploty
  Konst.Temp1    = 5.12/1024.0;
  Konst.Temp2    = 22.8/23.7*50.0 * 1.64;
  // konstanta pro prevod z cisla na kg/m3 pro vypocet hustoty vzduchu
  Konst.Density1 = 1127.7;
  Konst.Density2 = (1295.9 - 1127.7) / 40.0;

  // vypocet biasu senzoru dynamickeho tlaku
  Measure.DynamicBias = ((float)Dynamic) / ((float)NumDyn);
  // vypocet lokalniho tlaku
  Measure.LocalPress  = ((float)Static / ((float)NumStat)) * Konst.Static;
  // vypocet lokalni teploty
  Measure.LocalTemp   = ((((float)Temp / ((float)NumTemp)) * Konst.Temp1) - 0.5) * Konst.Temp2;
  
  // konstanty pro mereni vysky letu
  Konst.Height1   = 44330.0;
  Konst.Height2   = 0.19026;
  
  // konstanty pro kalibraci
  /*CalibStatic.statOff1   = -892.7358;
  CalibStatic.statOff2   = 900;
  CalibStatic.statCon.p0 = 0;
  CalibStatic.statCon.p1 = 2.322e-006;
  CalibStatic.statCon.p2 = 0.89748;
  CalibStatic.statCon.p3 = -0.0036989;
  
  CalibDynamic.statConM.p0 = 2.7524e-009;
  CalibDynamic.statConM.p1 = 5.9147e-006;
  CalibDynamic.statConM.p2 = 1.4552;
  CalibDynamic.statConM.p3 = -0.047218;
  CalibDynamic.statConP.p0 = -2.0072e-008;
  CalibDynamic.statConP.p1 = 3.7689e-005;
  CalibDynamic.statConP.p2 = 1.4285;
  CalibDynamic.statConP.p3 = -0.038079;
  
  Measure.LocalPress = ( CalibStatic.statCon.p1*(Measure.LocalPress+CalibStatic.statOff1)*(Measure.LocalPress+CalibStatic.statOff1) +
                       CalibStatic.statCon.p2*(Measure.LocalPress+CalibStatic.statOff1) + CalibStatic.statCon.p3 )
                     + CalibStatic.statOff2;    */
  
  LED0 = 1;
  LED1 = 1;
}

byte can_recv( can_message_t *pmess)
{
 byte i, tmp;
  
 if( CAN0RFLG & 0x01) 
 {
   pmess->id = CAN0RXIDR0 << 8;
   pmess->id += CAN0RXIDR1;
   if( pmess->id & 0x0010) 
   { 
     pmess->len = 0x80;
   }
   else
   {
     pmess->len = 0;
   }
  
   pmess->id = ( pmess->id >> 5) & 0x07ff;
                                                                                                    
   if( ( CAN0RXDLR & 0x0f) > 8) 
   {
     pmess->len |= 0x08;
     tmp = 8;
   }
   else	
   {
     tmp = CAN0RXDLR & 0x0f;
     pmess->len |= tmp;  
   }
   
   for( i=0; i < tmp; i++)
   {
     pmess->data[i] = (&CAN0RXDSR0)[i];
   }
   return 1;
  }
  return 0;
}

byte can_send( can_message_t *pmess)
{
  char i;
  unsigned short volatile *pireg;
 
  if( CAN0TFLG & 0x01)
  {
    pireg = ( unsigned short volatile *)( &CAN0TXIDR0);
    if( pmess->len & 0x80)
    { 
      *pireg = ( ( pmess->id << 5) & 0xffe0) | 0x0010;	// write id, rtr frame
    }
    else
    {
      *pireg = ( ( pmess->id << 5) & 0xffe0); // write id, data frame
    }
    pireg++;
    *pireg = 0;
    
    pmess->len &= 0x0f;
    for( i = 0; i < pmess->len; i++)
    {
      ( &CAN0TXDSR0)[i] = pmess->data[i];	  // write data
    }
    CAN0TXDLR = pmess->len; 	        // write dlc
    CAN0TFLG = 1;	 					  // start transmission
    return 1;
  }

  return 0;
}

void can_init(void)
{
  CAN0CTL0 |= INITRQ; // set initrq, this will also set initak
  while( !( CAN0CTL1 & INITAK)); // wait for init mode to occur

  CAN0CTL1 = CANE; // set cane just in case this is the first time after reset

  CAN0BTR0=0x01; // for 500kbps 0x03, for 100kbps 0x13
  CAN0BTR1=0x32; // for a can Baud of 500kbps at 16Mhz crystal

  // can id filter bits
  CAN0IDAR0=0x00;
  CAN0IDAR1=0x00;
  CAN0IDAR2=0x00;
  CAN0IDAR3=0x00;
  CAN0IDAR4=0x00;
  CAN0IDAR5=0x00;
  CAN0IDAR6=0x00;
  CAN0IDAR7=0x00;

  // can id filter mask
  CAN0IDMR0=0xff;
  CAN0IDMR1=0xff;
  CAN0IDMR2=0xff;
  CAN0IDMR3=0xff;
  CAN0IDMR4=0xff;
  CAN0IDMR5=0xff;
  CAN0IDMR6=0xff;
  CAN0IDMR7=0xff;

  CAN0IDAC = 0x10; // set filters to 2 16 bit acceptance filters

  CAN0CTL0 &= ~INITRQ; // clear initrq
  while (CAN0CTL1 & INITAK);

  CAN0CTL1 |= CANE; // set cane just in case this is the first time after reset
  CAN0RIER_RXFIE = 0; // enable receive interrupt

  CAN0TBSEL = 1;
}

/* END adc */
/*
** ###################################################################
**
**     This file was created by Processor Expert 3.02 [04.44]
**     for the Freescale HCS12X series of microcontrollers.
**
** ###################################################################
*/
